#include "statectrl.h"
using namespace STATECONTROL;

StateController::StateController()
{
    initController();
}

void StateController::initController()
{
    auCount=CALIBRATE;
    auCurr=0.0;
    auSave=0.0;
    auTimes=STATETIMES;
    auSign=1;
    stateNow=0;
    auRecal=false;
}

int StateController::stateRecognize(const double axialFroceNow)
{
    if(0==forceThre) return 0;
    if(auCount==CALIBRATE){
        auCurr+=axialFroceNow;
        auCount--;
        return -2;
    }else if(0==auCount){
        auCount--;
        return -3;
    }
    if(auCount>0){
        auCurr+=axialFroceNow;
        if(auCount--==1){
            auCurr/=CALIBRATE;
            if(!auRecal) auSave=auCurr;
            if(abs(axialFroceNow-auSave)>forceThre){
                auCount=CALIBRATE-1;
                auCount=axialFroceNow;
                auRecal=true;
            }
        }
        return -4;
    }else if(auCount<0){
        if(abs(axialFroceNow-auSave)>forceThre){
            if(auSign/(axialFroceNow-auSave) <0){
                stateNow++;
                auSign=axialFroceNow-auSave;
                auCount=CALIBRATE;
                auRecal=false;
                auCurr=0;
                return stateNow;
            }
        }
    }
    return -4;
}

int StateController::getAuTimes() const
{
    return auTimes;
}

void StateController::setAuTimes(int value)
{
    auTimes = value;
}

double StateController::getForceThre() const
{
    return forceThre;
}

void StateController::setForceThre(double value)
{
    forceThre = value;
}
